﻿using System.Collections;
using System.Collections.Generic;
using ROSBridgeLib.geometry_msgs;
using UnityEngine;
using CnControls;

public class NavLaunch : BaseLaunch
{


    [SerializeField]
    SimpleJoystick joystick;


   
    public override void OnClickMap(Vector3 positionInMap)
    {
       

//        Debug.Log("Set Goal At:"+positionInMap);

        positionInMap.z=0;

        RobotposeSubscriber robotPose =(RobotposeSubscriber) GetSubscriber(typeof(RobotposeSubscriber));

        NavGoalPublisher navGoal =(NavGoalPublisher)GetPublisher(typeof(NavGoalPublisher));

        if (robotPose != null && navGoal != null)
        {

            Vector3 robotLocalPos = robotPose.transform.localPosition;

            Vector3 direction = positionInMap - robotLocalPos;

            float angle = Mathf.Atan2(direction.y, direction.x) * Mathf.Rad2Deg;

            Publish(navGoal.topic,navGoal.PackageMsg(positionInMap,Quaternion.Euler(0,0,angle)));

        }

    }

    public override void OnEveryUpdate()
    {

       

        Vector2 inputVector = new Vector2(CnInputManager.GetAxis("Horizontal"), CnInputManager.GetAxis("Vertical"));


        if (inputVector.magnitude <0.2f)
        {
            return;
        }


        CmdVelPublisher cmd =(CmdVelPublisher)GetPublisher(typeof(CmdVelPublisher));

        Publish(cmd.topic,cmd.PackageMsg(inputVector));
            
    }
}
